#pragma once

#include "../ip/i_basic.h"

namespace idl
{
  template<typename DepthMapType, typename NeighborhoodType>
  inline DepthMapType i_interpolate_neighbor_w_missing_data(const int c[2], const DepthMapType *const *depth, const NeighborhoodType& nbhd, int width, int height, int min_nr_samples = 1, DepthMapType hole_val = (DepthMapType)0.0)
  {
    int i, col, row, nr_samples = 0;
    int cx = c[0];
    int cy = c[1];
    NeighborhoodType::DATATYPE sum_wts = (NeighborhoodType::DATATYPE)0.0;
    NeighborhoodType::DATATYPE sum_vls = (NeighborhoodType::DATATYPE)0.0;
    const NeighborhoodType::DATATYPE *weights = nbhd.get_weights();
    const Pair<int, int>* offsets = nbhd.get_offsets();
    const int region_size = nbhd.get_size();

    for (i = 0; i < region_size; ++i)
    {
      row = offsets[i].first  + cy; /*row offset first;*/
      col = offsets[i].second + cx; /*then column offset*/
      if (i_within_2d(col, row, 0, 0, width, height) && depth[row][col] != hole_val)
      {
        sum_wts += weights[i];
        sum_vls += (weights[i] * (NeighborhoodType::DATATYPE)depth[row][col]);
        nr_samples++;    
      }
    }
    return nr_samples >= i_max(1, min_nr_samples) ? i_div(sum_vls, sum_wts) : hole_val;
  }

  template<typename DepthMapType, typename NeighborhoodType>
  inline DepthMapType i_interpolate_neighbor_w_missing_data(const float c[2], const DepthMapType *const *depth, const NeighborhoodType& nbhd, int width, int height, int min_nr_samples = 1, DepthMapType hole_val = (DepthMapType)0.0)
  {
    int i, col, row, nr_samples = 0;
    int cx = i_round(c[0]);
    int cy = i_round(c[1]);
    NeighborhoodType::DATATYPE sum_wts = (NeighborhoodType::DATATYPE)0.0;
    NeighborhoodType::DATATYPE sum_vls = (NeighborhoodType::DATATYPE)0.0;
    const NeighborhoodType::DATATYPE *weights = nbhd.get_weights();
    const Pair<int, int>* offsets = nbhd.get_offsets();
    const int region_size = nbhd.get_size();

    for (i = 0; i < region_size; ++i)
    {
      row = offsets[i].first + cy;  /*row offset first;*/
      col = offsets[i].second + cx; /*then column offset*/
      if (i_within_2d(col, row, 0, 0, width, height) && depth[row][col] != hole_val)
      {
        sum_wts += weights[i];
        sum_vls += (weights[i] * (NeighborhoodType::DATATYPE)depth[row][col]);
        nr_samples++;
      }
    }
    return nr_samples >= i_max(1, min_nr_samples) ? i_div(sum_vls, sum_wts) : hole_val;
  }

  template<typename DepthMapType, typename NeighborhoodType>
  inline DepthMapType i_interpolate_neighbor_w_missing_data(const double c[2], const DepthMapType *const *depth, const NeighborhoodType& nbhd, int width, int height, int min_nr_samples = 1, DepthMapType hole_val = (DepthMapType)0.0)
  {
    int i, col, row, nr_samples = 0;
    int cx = i_round(c[0]);
    int cy = i_round(c[1]);
    NeighborhoodType::DATATYPE sum_wts = (NeighborhoodType::DATATYPE)0.0;
    NeighborhoodType::DATATYPE sum_vls = (NeighborhoodType::DATATYPE)0.0;
    const NeighborhoodType::DATATYPE *weights = nbhd.get_weights();
    const Pair<int, int>* offsets = nbhd.get_offsets();
    const int region_size = nbhd.get_size();

    for (i = 0; i < region_size; ++i)
    {
      row = offsets[i].first + cy;  /*row offset first;*/
      col = offsets[i].second + cx; /*then column offset*/
      if (i_within_2d(col, row, 0, 0, width, height) && depth[row][col] != hole_val)
      {
        sum_wts += weights[i];
        sum_vls += (weights[i] * (NeighborhoodType::DATATYPE)depth[row][col]);
        nr_samples++;
      }
    }
    return nr_samples >= i_max(1, min_nr_samples) ? i_div(sum_vls, sum_wts) : hole_val;
  }

  /*This routine samples the depth value from a (incomplete) depth map, "hole" regions have depth value "hole_val" on the depth map.
  If the query pixel has valid measurement value, return that value, 
  Else if the query pixel is a "hole", sample around a neighborhood "nbhd" and compute the weighted average depth value within that neighborhood*/
  template<typename DepthMapType, typename NeighborhoodType>
  inline DepthMapType i_sample_depthmap_w_missing_data(const int c[2], const DepthMapType *const *depth, const NeighborhoodType& nbhd, int width, int height, int min_nr_samples = 1, DepthMapType hole_val = (DepthMapType)0.0)
  {
    int x = c[0];
    int y = c[1];
    DepthMapType val = hole_val;
    if (i_within_2d(x, y, 0, 0, width, height))
    {
      val = depth[y][x];
      if (val != hole_val)
      {
        return val;
      }
      else /*the pixel of interest is a "hole"*/
      {
        val = i_interpolate_neighbor_w_missing_data(c, depth, nbhd, width, height, min_nr_samples, hole_val);
      }
    }
    return val;
  }

  template<typename DepthMapType, typename NeighborhoodType>
  inline DepthMapType i_sample_depthmap_w_missing_data(const float c[2], const DepthMapType *const *depth, const NeighborhoodType& nbhd, int width, int height, int min_nr_samples = 1, DepthMapType hole_val = (DepthMapType)0.0)
  {
    int x = i_round(c[0]);
    int y = i_round(c[1]);
    DepthMapType val = hole_val;
    if (i_within_2d(x, y, 0, 0, width, height))
    {
      val = depth[y][x];
      if (val != hole_val)
      {
        return val;
      }
      else /*the pixel of interest is a "hole"*/
      {
        val = i_interpolate_neighbor_w_missing_data(c, depth, nbhd, width, height, min_nr_samples, hole_val);
      }
    }
    return val;
  }

  template<typename DepthMapType, typename NeighborhoodType>
  inline DepthMapType i_sample_depthmap_w_missing_data(const double c[2], const DepthMapType *const *depth, const NeighborhoodType& nbhd, int width, int height, int min_nr_samples = 1, DepthMapType hole_val = (DepthMapType)0.0)
  {
    int x = i_round(c[0]);
    int y = i_round(c[1]);
    DepthMapType val = hole_val;
    if (i_within_2d(x, y, 0, 0, width, height))
    {
      val = depth[y][x];
      if (val != hole_val)
      {
        return val;
      }
      else /*the pixel of interest is a "hole"*/
      {
        val = i_interpolate_neighbor_w_missing_data(c, depth, nbhd, width, height, min_nr_samples, hole_val);
      }
    }
    return val;
  }
} /*namespace idl*/